#!/usr/bin/env python

# This should work as an TEMPLATE for all future moveit related python code

import sys
import rospy
from yaml import load
try:
    from yaml import CLoader as Loader
except ImportError:
    from yaml import Loader

import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import aubo_i5_hand_eye_cali.msg as cali_msg
import aubo_i5_hand_eye_cali.srv as cali_srv
import tf
import numpy
from threading import Thread


# This node accepts position commands of the manipulator with respect to
# the predefined table frame, and convert the command into base_link frame
# and send it to the robot to execute.

# note currently only pose vertical to the table is supported.

class MoveInTableNode(Thread):  #MoveInTableNode is a thread.

    def __init__(self):

        Thread.__init__(self)

        # read config files
        with open("../config/"+"aubo.yaml") as conf_file:
            config = load(conf_file, Loader=Loader)
            conf = config['hand_eye_conf']

            self.move_group_name = conf['move_group_name']           
            print "move_group_name: %s" % conf['move_group_name']
            self.cmd_ref_frame = conf['cmd_ref_frame']
            print "cmd_ref_frame: %s" % conf['cmd_ref_frame']
            self.world_frame = conf['world_frame']
            print "world_frame: %s" % conf['world_frame']            
            self.move_group_end_effector = conf['end_effector_name']
            print "move_group_end_effector: %s" % conf['end_effector_name']                  
            self.move_group_board_frame = conf['cali_board_name']
            print "move_group_board_frame: %s" % conf['cali_board_name']                  

        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()

        # the moveit group name defined according to the moveit config
        self.group = moveit_commander.MoveGroupCommander(self.move_group_name)
        self.group.set_planner_id("RRTConnectkConfigDefault")

        # create a DisplayTrajectory publisher for RVIZ to visualize
        self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)

        # sending planning command to ee_link
        self.group.set_end_effector_link(self.move_group_end_effector)

        self.frame_listener =  tf.TransformListener()

        pass

    def run(self):

        # listen to table frame broadcaster
        try:

            rospy.Service("move_arm_service",cali_srv.MoveArmQuaternionSrv, self.onMoveArmService)

            print "Waiting for command msgs.. "

            rospy.spin()

        except KeyboardInterrupt:
            pass
        except (tf.LookupException, tf.ConnectivityException):
            pass


        pass


# WARNING: all planning related function call all have to make sure the
# start state is the current state
    def onMoveArmService(self, req):

        pose_target = req.pose
        try:
            robot_current_state = self.robot.get_current_state()
            #rospy.logerr("Current_state: %s",robot_current_state.__str__())
            self.group.set_start_state(robot_current_state)
            self.group.set_pose_target(pose_target)
            plan = self.group.plan()

            MAX_TRYS = 100
            count = 0
            # if the plan result is not valid ( cost too much time or
            # is empty, replan it
            while not self.is_plan_checked(plan):
                plan = self.group.plan()
                count += 1
                if count == MAX_TRYS:
                    raise Exception("No plan found after trying dead hard.")

#            rospy.loginfo(plan.__str__())

            rospy.loginfo("* onMoveInTableverticalService called")

            display_trajectory = moveit_msgs.msg.DisplayTrajectory()
            display_trajectory.trajectory_start= self.robot.get_current_state()
            display_trajectory.trajectory.append(plan)

            self.display_trajectory_publisher.publish(display_trajectory)

            # accelerate the trajectory by a factor of 2
            traj = plan.joint_trajectory
            for point in traj.points:
                point.time_from_start = rospy.Duration.from_sec(point.time_from_start.to_sec()/2.5)

#                rospy.loginfo("Excute Plan and Block... ")
            self.group.execute(plan)
            rospy.logerr("Excution Done! ")
            return True

        except Exception as e:
            print e
            return False
            pass

        pass

    def is_plan_checked(self,plan):

        # the maximum time allowed for execution, in seconds
        ALLOWED_TIME = 10.0
        ALLOWED_VARIANCE = 0.3

        if not plan:
            rospy.logerr("### Empty Plan Checked ")
            return False
        elif type(plan) is not moveit_msgs.msg.RobotTrajectory:
            rospy.logerr("### Plan Type invalid ")
            return False
        elif plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1].time_from_start.to_sec() > ALLOWED_TIME:
            rospy.logerr("### Plan Time too long, giving up.")
            return False
        elif self.compute_variance(plan) > ALLOWED_VARIANCE:
            rospy.logerr("### Unexpected move may happen,give up.")
        else:
            rospy.logerr("### Planned Moving Time: %s", plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1].time_from_start.to_sec())
            return True

    def compute_variance(self,plan):

        total = plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1].time_from_start.to_sec()
        mean = total/(len(plan.joint_trajectory.points))
        durations = []
        total_difference = 0

        for index in range(len(plan.joint_trajectory.points)-2):
            duration = plan.joint_trajectory.points[index+1].time_from_start.to_sec()-plan.joint_trajectory.points[index].time_from_start.to_sec()
            difference = (duration - mean) * (duration - mean)
            total_difference += difference
        variance = total_difference/(len(plan.joint_trajectory.points))


        return variance


if __name__ == "__main__":
        # init rospy
        print "============== Starting Setup"
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('move_in_table_node')

        # wait for moveit stuff starting up
        #rospy.sleep(10)

        node = MoveInTableNode()

        node.start();

        try:
            while not rospy.is_shutdown():
                rospy.sleep(1)
        except (KeyboardInterrupt,rospy.ROSInterruptException):
            pass

        pass
